#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import PointCloud2, PointField
from std_msgs.msg import Int8
import numpy as np
import struct
import open3d as o3d
import time
import pandas as pd
import matplotlib.cm as cm
from threading import Thread, Lock, Event

def pointcloud2_to_numpy(msg: PointCloud2) -> np.ndarray:
    num_points = msg.width * msg.height; point_step = msg.point_step; data = msg.data; offsets = {}; dtypes = {PointField.INT8: 'b', PointField.UINT8: 'B', PointField.INT16: 'h', PointField.UINT16: 'H', PointField.INT32: 'i', PointField.UINT32: 'I', PointField.FLOAT32: 'f', PointField.FLOAT64: 'd'};
    for field in msg.fields:
        if field.name in ['x', 'y', 'z', 'intensity', 'reflectivity']:
            offsets[field.name] = field.offset; dtypes[field.name] = dtypes[field.datatype]
    if not all(k in offsets for k in ['x', 'y', 'z']): raise ValueError("PointCloud2消息缺少x, y, 或 z字段")
    has_intensity = 'intensity' in offsets or 'reflectivity' in offsets; intensity_key = 'intensity' if 'intensity' in offsets else 'reflectivity'
    points = [];
    for i in range(num_points):
        base_idx = i * point_step; x = struct.unpack_from(dtypes['x'], data, offset=base_idx + offsets['x'])[0]; y = struct.unpack_from(dtypes['y'], data, offset=base_idx + offsets['y'])[0]; z = struct.unpack_from(dtypes['z'], data, offset=base_idx + offsets['z'])[0]; intensity = 0.0
        if has_intensity: intensity = struct.unpack_from(dtypes[intensity_key], data, offset=base_idx + offsets[intensity_key])[0]
        points.append([x, y, z, intensity])
    return np.array(points, dtype=np.float32)

FIELDS_OUT = [PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1), PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1), PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1), PointField(name='intensity', offset=12, datatype=PointField.FLOAT32, count=1)]
POINT_STEP_OUT = 16

def create_pointcloud2_msg(header, points_array):
    pcl_msg = PointCloud2(); pcl_msg.header = header; pcl_msg.height = 1; pcl_msg.width = len(points_array); pcl_msg.fields = FIELDS_OUT; pcl_msg.is_bigendian = False; pcl_msg.point_step = POINT_STEP_OUT; pcl_msg.row_step = POINT_STEP_OUT * pcl_msg.width; pcl_msg.is_dense = False; buffer = bytearray(pcl_msg.row_step);
    for i, point in enumerate(points_array): struct.pack_into('ffff', buffer, i * POINT_STEP_OUT, point[0], point[1], point[2], point[3])
    pcl_msg.data = buffer
    return pcl_msg

class LidarProcessor(Node):
    def __init__(self):
        super().__init__('lidar_processor')
        self.declare_parameter('input_cloud', '/livox/lidar')
        input_topic = self.get_parameter('input_cloud').get_parameter_value().string_value
        self.sub = self.create_subscription(PointCloud2, input_topic, self.ros_callback, 10)
        self.pcl_pub = self.create_publisher(PointCloud2, '/livox/pointcloud2_processed', 10)
        self.elevation_map_pub = self.create_publisher(PointCloud2, '/elevation_map', 10)
        self.collision_pub = self.create_publisher(Int8, '/hook_collision_state', 10)
        self.get_logger().info(f"事件驱动多线程版本已启动。订阅: {input_topic}")
        
        ##################################################################
        # ---               【核心】可调节参数区域                     ---
        ##################################################################

        # --- 状态变量 (程序自动更新，无需调节) ---
        self.frame_count = 0; 
        self.estimated_ground_x = None
        self.estimated_hook_center = None
        # 碰撞检测状态
        self.collision_state = 0
        self.collision_state_counter = 0
        self.collision_geometries = []
        # [新增] 吊钩跟踪状态
        self.STATE_NO_TARGET = 0
        self.STATE_DETECTED = 1
        self.STATE_TRACKING = 2
        self.hook_tracking_state = self.STATE_NO_TARGET
        self.hook_lost_counter = 0

        # --- 1. 地面分割参数 ---
        self.enable_ground_removal = True        # [开关] 是否启用地面移除功能。
        self.ground_search_radius = 10.0         # [距离(米)] 地面搜索半径：在此水平半径内搜索地面点。
        self.ground_ransac_distance = 0.2        # [距离(米)] RANSAC地面容差：点距离拟合平面的最大距离。适用于坑洼地面可增大，平整地面可减小。
        self.ground_min_points_ratio = 0.05      # [比例(0-1)] 最低地面点比例：一个平面至少要占搜索区域总点数的5%，才被认为是有效地面。
        self.ground_level_smoothing_factor = 0.1 # [因子(0-1)] 地面深度平滑因子：用于稳定地面高度的估计值。值越小越稳定，但对地形变化响应越慢。

        # --- 2. 粗定位ROI参数 (用于寻找绳索) ---
        self.coarse_search_radius = 2.0          # [距离(米)] 粗搜索半径：定义用于寻找绳索的垂直圆柱体的水平半径。
        self.coarse_search_min_x = 0.3           # [距离(米)] 粗搜索最小深度：圆柱体的起始深度，用于避免检测到小车自身。
        self.coarse_search_max_x = 30.0          # [距离(米)] 粗搜索最大深度：圆柱体的结束深度，应设为塔吊的最大工作高度。
        
        # --- 3. 绳索平面检测参数 ---
        self.rope_plane_ransac_distance = 0.3    # [距离(米)] RANSAC绳索平面容差：点的距离小于此值才被认为是绳索的一部分。
        self.rope_min_points = 20                # [数量] 最少绳索点数：构成一个有效绳索平面所需的最少点数。

        # --- 4. 吊钩精炼 (Fine-Tuning) 与防抖参数 ---
        self.hook_position_smoothing_factor = 1  # [因子(0-1)] 吊钩位置平滑因子：用于稳定吊钩的追踪位置。值越小越稳定。
        self.hook_refinement_radius = 0.6        # [距离(米)] 精炼区域半径：以粗定位的吊钩末端为球心，进行最终聚类的球形区域半径。
        self.hook_refinement_cluster_eps = 0.4   # [距离(米)] 精炼聚类邻域半径：在精炼区域内进行DBSCAN聚类时的`eps`值。
        self.hook_refinement_min_points = 10     # [数量] 最少吊钩点数：在精炼区域内，构成一个最终吊钩簇所需的最小点数。
        self.hook_max_lost_frames = 3            # [帧数] 允许吊钩连续丢失的最大帧数。在此期间，系统会基于最后已知位置进行“跟踪”，防止闪烁。

        # --- 5. 高级过滤参数 (拓扑连接性) ---
        self.enable_ground_connected_filtering = True # [开关] 是否启用基于拓扑连接性的地面干扰物过滤。
        self.ground_cluster_eps = 0.4                 # [距离(米)] 用于识别地面物体的聚类邻域半径(`eps`)。
        self.ground_cluster_min_points = 10           # [数量] 构成一个地面干扰物所需的最小点数(`min_points`)。
        self.ground_touch_margin = 0.5                # [距离(米)] 聚类簇最低点离地面多近，就算“接触”地面。

        # --- 6. 诊断功能开关 ---
        self.enable_tilt_detection = True        # [开关] 是否计算并打印吊钩倾斜角度。

        # --- 7. 高程地图参数 ---
        self.enable_elevation_map = False        # [开关] 是否启用高程地图生成和发布功能。
        self.enable_map_visualization = False     # [开关] 是否弹出专门的高程地图可视化窗口。
        self.map_resolution = 0.5                # [距离(米)] 地图栅格大小，即地图的精细度。
        self.map_size_meters = 50.0              # [距离(米)] 地图的总边长。
        self.map_height_method = 'mean'          # [模式] 栅格高度计算方法 ('max', 'min', 'mean')。
        self.lidar_centric_blinding_radius = 1.0 # [距离(米)] 雷达中心屏蔽半径：消除传感器正下方的干扰。
        self.map_visualization_style = 'flat'  # [模式] 地图可视化风格 ('flat' 或 'pillar')。
        self.map_flat_colormap = 'terrain'       # [字符串] 'flat'模式下的颜色映射表。
        self.map_pillar_samples = 10             # [数量] 'pillar'模式下每个柱体由多少个点构成。
        self.map_num_cells = int(self.map_size_meters / self.map_resolution)

        # --- 8. 可视化视角参数 ---
        self.vis_main_view = {"front": [-0.5, -0.8, -0.6], "lookat": [15.0, 0.0, 0.0], "up": [-1.0, 0.0, 0.0], "zoom": 0.6}

        # --- 9. 性能优化参数 ---
        self.enable_selective_downsampling = True # [开关] 是否启用选择性降采样来提速。
        self.downsampling_voxel_size = 0.2        # [距离(米)] 降采样的体素大小。值越大，降采样越狠，但细节损失越多。
        
        # --- 10. 碰撞检测参数 (Collision Detection) ---
        self.enable_collision_detection = True      # [开关] 是否启用碰撞检测功能。
        self.collision_body_dimensions = np.array([1.6, 1.6, 1.6]) # [尺寸(米)] 代表吊钩/货物的碰撞体尺寸(X,Y,Z)。
        self.critical_zone_padding = 0.5            # [距离(米)] 危险区边界：在碰撞体之外扩展此距离。
        self.warning_zone_padding = 1.5             # [距离(米)] 警戒区边界：在碰撞体之外扩展此距离。
        self.critical_points_threshold = 10         # [数量] 触发“危险”状态所需的最少障碍物点数。
        self.warning_points_threshold = 20          # [数量] 触发“警告”状态所需的最少障碍物点数。
        self.debounce_escalate_frames = 3           # [帧数] 状态升级防抖：必须连续N帧检测到更高级别风险，才会升级状态。
        self.debounce_descend_frames = 2            # [帧数] 状态降级防抖：必须连续N帧检测到更低级别风险，才会降级状态。

        # --- 多线程核心数据结构 ---
        self.latest_msg = None; self.msg_lock = Lock(); self.new_msg_event = Event()
        self.processed_data = {"main_points_o3d": None, "main_colors_o3d": None, "map_points_o3d": None, "map_colors_o3d": None, "collision_geometries": []}
        self.processed_data_lock = Lock(); self.new_processed_data_event = Event()
        self.running = True
        self.processing_thread = Thread(target=self.processing_loop); self.visualization_thread = Thread(target=self.visualization_loop)
        self.processing_thread.start(); self.visualization_thread.start()

    def ros_callback(self, msg: PointCloud2):
        with self.msg_lock: self.latest_msg = msg
        self.new_msg_event.set()

    def processing_loop(self):
        while self.running:
            self.new_msg_event.wait();
            if not self.running: break
            with self.msg_lock: msg = self.latest_msg
            self.new_msg_event.clear()
            if msg is None: continue
            
            start_time = time.time();
            try: points_array = pointcloud2_to_numpy(msg)
            except (ValueError, KeyError) as e: continue
            if points_array.shape[0] == 0: continue
            
            self.get_logger().info(f"--- [帧 {self.frame_count+1}] ---")
            
            if self.enable_selective_downsampling:
                num_original_points_pre_downsample = len(points_array)
                dist_yz = np.sqrt(points_array[:, 1]**2 + points_array[:, 2]**2)
                roi_mask = (dist_yz <= self.coarse_search_radius) & (points_array[:, 0] >= self.coarse_search_min_x) & (points_array[:, 0] <= self.coarse_search_max_x)
                points_in_roi = points_array[roi_mask]; points_outside_roi = points_array[~roi_mask]
                if len(points_outside_roi) > 0:
                    pcd_outside = o3d.geometry.PointCloud(); pcd_outside.points = o3d.utility.Vector3dVector(points_outside_roi[:, :3])
                    pcd_outside_downsampled = pcd_outside.voxel_down_sample(self.downsampling_voxel_size)
                    points_outside_downsampled = np.asarray(pcd_outside_downsampled.points)
                    num_downsampled = len(points_outside_downsampled)
                    points_outside_downsampled_with_intensity = np.hstack([points_outside_downsampled, np.zeros((num_downsampled, 1), dtype=np.float32)])
                    points_array = np.vstack((points_in_roi, points_outside_downsampled_with_intensity))
                    self.get_logger().info(f"    - 降采样: 点数从 {num_original_points_pre_downsample} 减少到 {len(points_array)}")

            num_original_points = len(points_array); point_labels = np.zeros(num_original_points, dtype=int); non_ground_mask = np.ones(num_original_points, dtype=bool)
            
            if self.enable_ground_removal:
                dist_yz_all = np.sqrt(points_array[:, 1]**2 + points_array[:, 2]**2); ground_search_mask = (dist_yz_all <= self.ground_search_radius); points_for_ground_search = points_array[ground_search_mask]
                if len(points_for_ground_search) > 100:
                    pcd_ground_search = o3d.geometry.PointCloud(); pcd_ground_search.points = o3d.utility.Vector3dVector(points_for_ground_search[:, :3]); plane_model, inliers = pcd_ground_search.segment_plane(distance_threshold=self.ground_ransac_distance, ransac_n=3, num_iterations=100)
                    normal_vector = plane_model[:3]; is_horizontal_plane = np.abs(normal_vector[0]) > 0.9
                    if len(inliers) > len(points_for_ground_search) * self.ground_min_points_ratio and is_horizontal_plane:
                        original_indices_of_ground_search = np.where(ground_search_mask)[0]; ground_indices = original_indices_of_ground_search[inliers]; point_labels[ground_indices] = 1; non_ground_mask[ground_indices] = False
                        current_ground_x = np.median(points_array[ground_indices, 0]);
                        if self.estimated_ground_x is None: self.estimated_ground_x = current_ground_x
                        else: self.estimated_ground_x = self.ground_level_smoothing_factor * current_ground_x + (1 - self.ground_level_smoothing_factor) * self.estimated_ground_x

            if self.estimated_ground_x is not None:
                self.get_logger().info(f"    - 估算地面距离: {self.estimated_ground_x:.2f} 米")

            if self.enable_ground_connected_filtering and self.estimated_ground_x is not None:
                initial_non_ground_indices = np.where(non_ground_mask)[0]
                if initial_non_ground_indices.size > 0:
                    points_for_clustering = points_array[initial_non_ground_indices, :3]
                    pcd_for_clustering = o3d.geometry.PointCloud(); pcd_for_clustering.points = o3d.utility.Vector3dVector(points_for_clustering)
                    cluster_labels = np.array(pcd_for_clustering.cluster_dbscan(eps=self.ground_cluster_eps, min_points=self.ground_cluster_min_points, print_progress=False))
                    ground_connected_labels = set()
                    unique_labels = np.unique(cluster_labels)
                    for label in unique_labels:
                        if label == -1: continue
                        points_in_cluster = points_for_clustering[cluster_labels == label]
                        max_x_in_cluster = np.max(points_in_cluster[:, 0])
                        if max_x_in_cluster > (self.estimated_ground_x - self.ground_touch_margin): ground_connected_labels.add(label)
                    if ground_connected_labels:
                        is_ground_connected = np.isin(cluster_labels, list(ground_connected_labels))
                        global_indices_to_remove = initial_non_ground_indices[is_ground_connected]
                        non_ground_mask[global_indices_to_remove] = False
                        self.get_logger().info(f"    - 拓扑过滤: 移除了 {len(ground_connected_labels)} 个与地面连接的聚类 ({np.sum(is_ground_connected)} 个点)。")

            non_ground_points = points_array[non_ground_mask]; original_indices_of_non_ground = np.where(non_ground_mask)[0]
            dist_yz_non_ground = np.sqrt(non_ground_points[:, 1]**2 + non_ground_points[:, 2]**2); coarse_roi_mask_local = (dist_yz_non_ground <= self.coarse_search_radius) & (non_ground_points[:, 0] >= self.coarse_search_min_x) & (non_ground_points[:, 0] <= self.coarse_search_max_x)
            points_in_coarse_roi = non_ground_points[coarse_roi_mask_local]; coarse_roi_indices_global = original_indices_of_non_ground[coarse_roi_mask_local]; point_labels[coarse_roi_indices_global] = 4
            
            is_hook_valid_this_frame = False
            if len(points_in_coarse_roi) > self.rope_min_points:
                pcd_coarse_roi = o3d.geometry.PointCloud(); pcd_coarse_roi.points = o3d.utility.Vector3dVector(points_in_coarse_roi[:, :3]); plane_model, inliers = pcd_coarse_roi.segment_plane(distance_threshold=self.rope_plane_ransac_distance, ransac_n=3, num_iterations=100)
                normal_vector = plane_model[:3]; is_vertical_plane = np.abs(normal_vector[0]) < 0.1
                
                if len(inliers) > self.rope_min_points and is_vertical_plane:
                    rope_plane_indices_in_coarse_roi = inliers; rope_plane_indices_local = np.where(coarse_roi_mask_local)[0][rope_plane_indices_in_coarse_roi]; rope_plane_indices_global = original_indices_of_non_ground[rope_plane_indices_local]
                    rope_plane_points = non_ground_points[rope_plane_indices_local]
                    point_labels[rope_plane_indices_global] = 3
                    if self.enable_tilt_detection: self._calculate_rope_tilt(rope_plane_points)
                    
                    max_x_rope = np.max(rope_plane_points[:, 0]); coarse_hook_points = rope_plane_points[rope_plane_points[:, 0] >= (max_x_rope - 0.3)]
                    
                    #####################################################################
                    # ---     【核心逻辑】吊钩检测、验证与跟踪状态机 (重构部分)        ---
                    #####################################################################
                    if len(coarse_hook_points) > 0:
                        # 仅当estimated_hook_center不存在时，才用粗略点进行初始化
                        if self.estimated_hook_center is None:
                            self.estimated_hook_center = np.mean(coarse_hook_points[:, :3], axis=0)

                        distances_to_stable_center = np.linalg.norm(non_ground_points[:, :3] - self.estimated_hook_center, axis=1); refinement_mask_local = (distances_to_stable_center <= self.hook_refinement_radius)
                        points_for_refinement = non_ground_points[refinement_mask_local]
                        
                        if len(points_for_refinement) > self.hook_refinement_min_points:
                            pcd_refinement = o3d.geometry.PointCloud(); pcd_refinement.points = o3d.utility.Vector3dVector(points_for_refinement[:, :3]); cluster_labels = np.array(pcd_refinement.cluster_dbscan(eps=self.hook_refinement_cluster_eps, min_points=self.hook_refinement_min_points, print_progress=False))
                            
                            if cluster_labels.max() > -1:
                                unique_labels, counts = np.unique(cluster_labels[cluster_labels > -1], return_counts=True); final_hook_label = unique_labels[counts.argmax()]
                                final_hook_mask_in_refinement = (cluster_labels == final_hook_label); original_indices_of_refinement = np.where(refinement_mask_local)[0]; final_hook_indices_local = original_indices_of_refinement[final_hook_mask_in_refinement]
                                final_hook_indices_global = original_indices_of_non_ground[final_hook_indices_local]

                                is_valid_hook = True 
                                if self.estimated_ground_x is not None and final_hook_indices_global.size > 0:
                                    max_hook_x = np.max(points_array[final_hook_indices_global, 0])
                                    if max_hook_x > (self.estimated_ground_x - 1.0): is_valid_hook = False
                                
                                if is_valid_hook and final_hook_indices_global.size > 0:
                                    hook_center = np.mean(points_array[final_hook_indices_global, :3], axis=0)
                                    min_dist_to_rope = np.min(np.linalg.norm(rope_plane_points[:, :3] - hook_center, axis=1))
                                    if min_dist_to_rope > 0.5: is_valid_hook = False
                                
                                if is_valid_hook and final_hook_indices_global.size > 0:
                                    is_hook_valid_this_frame = True # 标记当前帧检测成功
                                    point_labels[final_hook_indices_global] = 2 

                                    # 1. 成功路径：更新状态机并重置计数器
                                    self.hook_tracking_state = self.STATE_DETECTED
                                    self.hook_lost_counter = 0

                                    # 2. 关键：仅在成功时，用更精确的精炼中心来更新位置估计
                                    current_refined_hook_center = np.mean(points_array[final_hook_indices_global, :3], axis=0)
                                    self.estimated_hook_center = self.hook_position_smoothing_factor * current_refined_hook_center + (1 - self.hook_position_smoothing_factor) * self.estimated_hook_center

                                    # 3. 日志记录
                                    final_hook_depth = np.max(points_array[final_hook_indices_global, 0])
                                    if self.estimated_ground_x is not None:
                                        height_from_ground = self.estimated_ground_x - final_hook_depth
                                        self.get_logger().info(f"    - 吊钩高度(距地面): {height_from_ground:.2f} 米")

            # --- 吊钩跟踪状态机更新 ---
            if not is_hook_valid_this_frame:
                if self.hook_tracking_state in [self.STATE_DETECTED, self.STATE_TRACKING]:
                    self.hook_lost_counter += 1
                    if self.hook_lost_counter > self.hook_max_lost_frames:
                        # 真正丢失
                        self.hook_tracking_state = self.STATE_NO_TARGET
                        self.estimated_hook_center = None # 清除位置
                        self.get_logger().warning("    - 吊钩目标丢失。")
                    else:
                        # 进入跟踪状态
                        self.hook_tracking_state = self.STATE_TRACKING
                        self.get_logger().info(f"    - 吊钩暂时丢失，进入跟踪模式... ({self.hook_lost_counter}/{self.hook_max_lost_frames})")
                else:
                    self.hook_tracking_state = self.STATE_NO_TARGET

            # --- 碰撞检测逻辑 ---
            if self.hook_tracking_state in [self.STATE_DETECTED, self.STATE_TRACKING]:
                if self.enable_collision_detection:
                    obstacle_mask = (point_labels != 1) & (point_labels != 2) & (point_labels != 3)
                    obstacle_points = points_array[obstacle_mask, :3]
                    self._update_collision_detection(obstacle_points, self.estimated_hook_center)
            else: # 如果没有目标，则清除碰撞检测
                self._update_collision_detection(np.array([]), None)

            # --- 根据状态决定颜色 ---
            final_colors = np.zeros((num_original_points, 3)); 
            final_colors[point_labels == 0] = [0.25, 0.25, 0.25];
            final_colors[point_labels == 1] = [0.0, 0.5, 0.0];
            final_colors[point_labels == 4] = [0.5, 0.5, 0.5];
            final_colors[point_labels == 3] = [0.0, 0.5, 1.0];
            if self.hook_tracking_state in [self.STATE_DETECTED, self.STATE_TRACKING]:
                if self.collision_state == 0:   final_colors[point_labels == 2] = [1.0, 0.0, 0.0] # 安全-红
                elif self.collision_state == 1: final_colors[point_labels == 2] = [1.0, 1.0, 0.0] # 警告-黄
                else:                           final_colors[point_labels == 2] = [1.0, 0.0, 1.0] # 危险-洋红

            map_pcd_array = None
            if self.enable_elevation_map: 
                map_pcd_array = self.generate_and_publish_elevation_map(points_array, point_labels, msg.header)
            
            with self.processed_data_lock:
                self.processed_data["main_points_o3d"] = o3d.utility.Vector3dVector(points_array[:, :3])
                self.processed_data["main_colors_o3d"] = o3d.utility.Vector3dVector(final_colors)
                self.processed_data["collision_geometries"] = self.collision_geometries
                map_points_o3d, map_colors_o3d = None, None
                if map_pcd_array is not None and len(map_pcd_array) > 0:
                    flat_colors_np = self._colorize_flat_map(map_pcd_array)
                    if self.map_visualization_style == 'pillar':
                        map_points_np, map_colors_np = self._create_pillar_map(map_pcd_array, flat_colors_np)
                        if len(map_points_np) > 0:
                            map_points_o3d = o3d.utility.Vector3dVector(map_points_np)
                            map_colors_o3d = o3d.utility.Vector3dVector(map_colors_np)
                    else:
                        map_points_o3d = o3d.utility.Vector3dVector(map_pcd_array)
                        map_colors_o3d = o3d.utility.Vector3dVector(flat_colors_np)
                self.processed_data["map_points_o3d"] = map_points_o3d
                self.processed_data["map_colors_o3d"] = map_colors_o3d
            
            self.new_processed_data_event.set()
            pcl_msg = create_pointcloud2_msg(msg.header, points_array)
            self.pcl_pub.publish(pcl_msg)
            process_time = (time.time() - start_time) * 1000
            self.get_logger().info(f"    - 处理耗时: {process_time:.2f}ms\n")
            self.frame_count += 1
    
    def visualization_loop(self):
        pcd_main = o3d.geometry.PointCloud()
        vis_main = o3d.visualization.Visualizer()
        vis_main.create_window(window_name='Main View')
        opt_main = vis_main.get_render_option()
        opt_main.point_size = 3.0
        opt_main.background_color = np.array([0, 0, 0])
        is_first_frame = True
        
        collision_geoms_o3d = {
            'body': o3d.geometry.LineSet(),
            'critical': o3d.geometry.LineSet(),
            'warning': o3d.geometry.LineSet()
        }
        geoms_in_visualizer = {name: False for name in collision_geoms_o3d}

        pcd_map = o3d.geometry.PointCloud()
        vis_map = o3d.visualization.Visualizer()
        if self.enable_map_visualization:
            vis_map.create_window(window_name='Elevation Map View')
            opt_map = vis_map.get_render_option()
            opt_map.point_size = 5.0
            opt_map.background_color = np.array([0.1, 0.1, 0.1])
        is_first_map_frame = True

        while self.running:
            self.new_processed_data_event.wait(timeout=0.1)
            if not self.running: break
            self.new_processed_data_event.clear()
            
            with self.processed_data_lock:
                main_points_o3d = self.processed_data["main_points_o3d"]
                main_colors_o3d = self.processed_data["main_colors_o3d"]
                map_points_o3d = self.processed_data["map_points_o3d"]
                map_colors_o3d = self.processed_data["map_colors_o3d"]
                collision_geometries_to_draw = self.processed_data["collision_geometries"]
            
            if main_points_o3d is not None and len(main_points_o3d) > 0:
                pcd_main.points = main_points_o3d
                pcd_main.colors = main_colors_o3d
                if is_first_frame:
                    vis_main.add_geometry(pcd_main, reset_bounding_box=True)
                    ctr = vis_main.get_view_control()
                    ctr.set_front(self.vis_main_view["front"])
                    ctr.set_lookat(self.vis_main_view["lookat"])
                    ctr.set_up(self.vis_main_view["up"])
                    ctr.set_zoom(self.vis_main_view["zoom"])
                    is_first_frame = False
                else:
                    vis_main.update_geometry(pcd_main)

            if self.enable_collision_detection:
                geom_data_map = {geom['name']: geom['geometry'] for geom in collision_geometries_to_draw}
                for name, geom_obj in collision_geoms_o3d.items():
                    if name in geom_data_map:
                        new_geom_data = geom_data_map[name]
                        geom_obj.points = new_geom_data.points
                        geom_obj.lines = new_geom_data.lines
                        geom_obj.colors = new_geom_data.colors
                        if not geoms_in_visualizer[name]:
                            vis_main.add_geometry(geom_obj, reset_bounding_box=False)
                            geoms_in_visualizer[name] = True
                        else:
                            vis_main.update_geometry(geom_obj)
                    else:
                        if not geom_obj.is_empty():
                           geom_obj.clear()
                           vis_main.update_geometry(geom_obj)

            if self.enable_map_visualization:
                if map_points_o3d is not None and len(map_points_o3d) > 0:
                    pcd_map.points = map_points_o3d; pcd_map.colors = map_colors_o3d
                    if is_first_map_frame:
                        vis_map.add_geometry(pcd_map); ctr = vis_map.get_view_control(); ctr.set_front([-1, 0, 0]); ctr.set_up([0, 0, 1]); is_first_map_frame = False
                    else: vis_map.update_geometry(pcd_map)
                elif not is_first_map_frame:
                    pcd_map.clear(); vis_map.update_geometry(pcd_map)
            
            if not vis_main.poll_events(): self.running = False
            vis_main.update_renderer()

            if self.enable_map_visualization and not is_first_map_frame:
                if not vis_map.poll_events(): self.running = False
                vis_map.update_renderer()

        vis_main.destroy_window()
        if self.enable_map_visualization and vis_map.get_window_handle() is not None:
            vis_map.destroy_window()

    def destroy_node(self):
        self.get_logger().info("正在关闭节点和所有线程...")
        self.running = False; self.new_msg_event.set(); self.new_processed_data_event.set()
        if hasattr(self, 'processing_thread') and self.processing_thread.is_alive(): self.processing_thread.join()
        if hasattr(self, 'visualization_thread') and self.visualization_thread.is_alive(): self.visualization_thread.join()
        super().destroy_node()
        self.get_logger().info("节点已成功关闭。")

    def _update_collision_detection(self, obstacle_points, hook_center):
        current_frame_state = 0
        self.collision_geometries = [] # 默认清空

        if hook_center is not None and obstacle_points.shape[0] > 0:
            half_dims = self.collision_body_dimensions / 2.0
            body_min = hook_center - half_dims
            body_max = hook_center + half_dims
            
            critical_min = body_min - self.critical_zone_padding
            critical_max = body_max + self.critical_zone_padding

            warning_min = body_min - self.warning_zone_padding
            warning_max = body_max + self.warning_zone_padding
            
            in_critical_zone_mask = (obstacle_points[:, 0] >= critical_min[0]) & (obstacle_points[:, 0] <= critical_max[0]) & (obstacle_points[:, 1] >= critical_min[1]) & (obstacle_points[:, 1] <= critical_max[1]) & (obstacle_points[:, 2] >= critical_min[2]) & (obstacle_points[:, 2] <= critical_max[2])
            in_warning_zone_mask = (obstacle_points[:, 0] >= warning_min[0]) & (obstacle_points[:, 0] <= warning_max[0]) & (obstacle_points[:, 1] >= warning_min[1]) & (obstacle_points[:, 1] <= warning_max[1]) & (obstacle_points[:, 2] >= warning_min[2]) & (obstacle_points[:, 2] <= warning_max[2])
            
            only_in_warning_mask = in_warning_zone_mask & ~in_critical_zone_mask
            num_critical_points = np.sum(in_critical_zone_mask)
            num_warning_points = np.sum(only_in_warning_mask)

            if num_critical_points >= self.critical_points_threshold: current_frame_state = 2
            elif num_warning_points >= self.warning_points_threshold: current_frame_state = 1
            
            body_box = o3d.geometry.AxisAlignedBoundingBox(min_bound=body_min, max_bound=body_max)
            critical_box = o3d.geometry.AxisAlignedBoundingBox(min_bound=critical_min, max_bound=critical_max)
            warning_box = o3d.geometry.AxisAlignedBoundingBox(min_bound=warning_min, max_bound=warning_max)
            
            body_lines = o3d.geometry.LineSet.create_from_axis_aligned_bounding_box(body_box); body_lines.paint_uniform_color([1.0, 0.0, 0.0])
            critical_lines = o3d.geometry.LineSet.create_from_axis_aligned_bounding_box(critical_box); critical_lines.paint_uniform_color([1.0, 0.0, 1.0])
            warning_lines = o3d.geometry.LineSet.create_from_axis_aligned_bounding_box(warning_box); warning_lines.paint_uniform_color([1.0, 1.0, 0.0])
            self.collision_geometries = [{'name': 'body', 'geometry': body_lines}, {'name': 'critical', 'geometry': critical_lines}, {'name': 'warning', 'geometry': warning_lines}]

        if current_frame_state > self.collision_state:
            if current_frame_state == self.collision_state + 1 or (current_frame_state > self.collision_state +1): self.collision_state_counter += 1
            else: self.collision_state_counter = 1
            if self.collision_state_counter >= self.debounce_escalate_frames:
                self.collision_state = min(current_frame_state, 2)
                if self.collision_state == 1: self.get_logger().warning("碰撞状态升级 -> [警告]")
                if self.collision_state == 2: self.get_logger().error("碰撞状态升级 -> [危险]")
                self.collision_state_counter = 0
        elif current_frame_state < self.collision_state:
            self.collision_state_counter += 1
            if self.collision_state_counter >= self.debounce_descend_frames:
                self.collision_state = current_frame_state
                if self.collision_state == 1: self.get_logger().warning("碰撞状态降级 -> [警告]")
                if self.collision_state == 0: self.get_logger().info("碰撞状态降级 -> [安全]")
                self.collision_state_counter = 0
        else: self.collision_state_counter = 0
        state_msg = Int8(); state_msg.data = self.collision_state; self.collision_pub.publish(state_msg)

    def _calculate_rope_tilt(self, rope_points):
        if len(rope_points) < 10: return
        centroid = np.mean(rope_points[:, :3], axis=0); centered_points = rope_points[:, :3] - centroid; cov_matrix = np.cov(centered_points, rowvar=False)
        _, _, vh = np.linalg.svd(cov_matrix); principal_direction = vh[0, :]
        vertical_axis = np.array([1.0, 0.0, 0.0]); cos_angle = np.abs(np.dot(principal_direction, vertical_axis)); cos_angle = np.clip(cos_angle, -1.0, 1.0)
        angle_rad = np.arccos(cos_angle); angle_deg = np.degrees(angle_rad)
        self.get_logger().info(f"    - 吊钩倾斜角度: {angle_deg:.2f} 度")

    def _colorize_flat_map(self, map_pcd_array):
        heights = map_pcd_array[:, 0]; min_h, max_h = np.min(heights), np.max(heights)
        if max_h == min_h: normalized_heights = np.full_like(heights, 0.5)
        else: normalized_heights = (heights - min_h) / (max_h - min_h)
        colormap = cm.get_cmap(self.map_flat_colormap); colors = colormap(normalized_heights)[:, :3]
        return colors

    def _create_pillar_map(self, map_pcd_array, map_colors_array):
        if self.estimated_ground_x is None: return np.array([]), np.array([])
        all_pillar_points, all_pillar_colors = [], []
        for i in range(len(map_pcd_array)):
            height, y, z = map_pcd_array[i]
            pillar_top_color = map_colors_array[i]
            pillar_x = np.linspace(height, self.estimated_ground_x, self.map_pillar_samples)
            pillar_y = np.full(self.map_pillar_samples, y)
            pillar_z = np.full(self.map_pillar_samples, z)
            pillar_points = np.vstack([pillar_x, pillar_y, pillar_z]).T
            all_pillar_points.append(pillar_points)
            pillar_colors = np.tile(pillar_top_color, (self.map_pillar_samples, 1))
            pillar_colors[0, :] = [1.0, 0.0, 0.0]
            all_pillar_colors.append(pillar_colors)
        if not all_pillar_points: return np.array([]), np.array([])
        return np.vstack(all_pillar_points), np.vstack(all_pillar_colors)

    def generate_and_publish_elevation_map(self, points, labels, header):
        map_points_mask = (labels == 1) | (labels == 0) | (labels == 4) | (labels == 3)
        map_points = points[map_points_mask]
        if len(map_points) < 10: return None
        y_coords = map_points[:, 1]; z_coords = map_points[:, 2]; x_heights = map_points[:, 0]
        center_y = np.median(y_coords); center_z = np.median(z_coords)
        half_size = self.map_size_meters / 2.0
        relative_y = y_coords - center_y; relative_z = z_coords - center_z
        col_indices = ((relative_y + half_size) / self.map_resolution).astype(int); row_indices = ((relative_z + half_size) / self.map_resolution).astype(int)
        valid_indices_mask = (col_indices >= 0) & (col_indices < self.map_num_cells) & (row_indices >= 0) & (row_indices < self.map_num_cells)
        if not np.any(valid_indices_mask): return None
        df = pd.DataFrame({'row': row_indices[valid_indices_mask], 'col': col_indices[valid_indices_mask], 'height': x_heights[valid_indices_mask]})
        if self.map_height_method == 'max': grid_heights = df.groupby(['row', 'col'])['height'].max()
        elif self.map_height_method == 'min': grid_heights = df.groupby(['row', 'col'])['height'].min()
        else: grid_heights = df.groupby(['row', 'col'])['height'].mean()
        if grid_heights.empty: return None
        map_grid_points = []
        for (r, c), h in grid_heights.items():
            y_rel = (c + 0.5) * self.map_resolution - half_size; z_rel = (r + 0.5) * self.map_resolution - half_size
            y_world = y_rel + center_y; z_world = z_rel + center_z
            map_grid_points.append([h, y_world, z_world])
        if not map_grid_points: return None
        map_pcd_array = np.array(map_grid_points, dtype=np.float32)
        
        if self.lidar_centric_blinding_radius > 0 and self.estimated_ground_x is not None:
            distances_from_lidar = np.sqrt(map_pcd_array[:, 1]**2 + map_pcd_array[:, 2]**2)
            blinding_mask = distances_from_lidar <= self.lidar_centric_blinding_radius
            map_pcd_array[blinding_mask, 0] = self.estimated_ground_x

        publish_array = np.hstack([map_pcd_array, map_pcd_array[:, [0]]])
        map_msg = create_pointcloud2_msg(header, publish_array)
        self.elevation_map_pub.publish(map_msg)
        return map_pcd_array

def main(args=None):
    rclpy.init(args=args); node = LidarProcessor()
    try: rclpy.spin(node)
    except KeyboardInterrupt: node.get_logger().info("捕获到Ctrl+C，开始关闭...")
    finally: node.destroy_node(); rclpy.shutdown()

if __name__ == '__main__':
    main()